# -*- coding: utf-8 -*-
# 声明文件编码为 UTF-8，确保支持中文字符和日志文件中的中文输出

# 导入必要的 Python 库
import numpy as np  # 用于数值计算，例如数组操作和数学运算
import math  # 提供数学函数，如三角函数、角度转换等
import gym  # OpenAI Gym 库，用于创建强化学习环境
from gym import spaces  # Gym 的子模块，用于定义动作空间和观测空间
from geopy.distance import geodesic  # 用于计算地球上两点之间的地理距离
from getDCSdata import get_dcs_data  # 自定义模块，从 DCS（数字战斗模拟器）获取飞行数据
from getDCSdata import is_crashed    # 坠毁标志

from vjoy_control import *
 # 自定义模块，控制虚拟摇杆（vJoy）以模拟飞行操作
import logging  # 用于记录日志，便于调试和监控程序运行R
import time  # 用于控制时间延迟，例如等待 DCS 数据
import matplotlib.pyplot as plt  # 用于绘制飞行轨迹和高度曲线
import csv  # 用于读写 CSV 文件，保存飞行数据和训练日志
import os  # 用于文件和目录操作，例如创建结果目录
import sys  # 用于系统相关操作，例如标准输出

# 日志配置：设置日志格式和输出方式
# 日志用于记录程序运行的关键信息，便于调试和分析
logging.basicConfig(
    level=logging.INFO,  # 设置日志级别为 INFO，记录信息、警告和错误
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',  # 日志格式：时间-模块名-级别-消息
    handlers=[
        logging.FileHandler("results/descent_env.log", encoding='utf-8'),  # 将日志保存到文件
        logging.StreamHandler(sys.stdout)  # 将日志输出到控制台
    ]
)
logger = logging.getLogger(__name__)  # 获取当前模块的日志记录器

# 定义 DescentEnv 类，继承自 gym.Env，用于创建强化学习环境
# 该环境模拟飞机在 DCS 中沿下滑道着陆的过程
class DescentEnv(gym.Env):
    # 跑道参数
    # 这些参数用于计算理想下滑路径和判断飞机状态
    # RUNWAY_LAT = 41.927222  # 跑道纬度（单位：度）  科布列季机场
    # RUNWAY_LONG = 41.849722  # 跑道经度（单位：度）  科布列季机场
    RUNWAY_LAT_start = 41.616389  # 跑道纬度（单位：度）  巴统机场跑道起点
    RUNWAY_LONG_start = 41.589444  # 跑道经度（单位：度）  巴统机场跑道起点
    RUNWAY_LAT_end = 41.601944  # 跑道纬度（单位：度）  巴统机场跑道终点
    RUNWAY_LONG_end = 41.611389  # 跑道经度（单位：度）  巴统机场跑道终点
    # RUNWAY_ALT = 18.0  # 跑道海拔高度（单位：米）  科布列季机场
    RUNWAY_ALT = 10.0  # 跑道海拔高度（单位：米）   巴统机场跑道
    APPROACH_SPEED = 79.17  # 进近速度（单位：米/秒）
    LANDING_SPEED = 65.28  # 着陆速度（单位：米/秒）
    RUNWAY_HEADING = 126  # 跑道航向（单位：度，顺时针相对于正北）  巴统机场跑道
    TRUE_HEADING = 70.0  # 真实航向，与跑道航向一致
    GLIDE_ANGLE = math.radians(3.0)  # 下滑角（3度，转换为弧度）
    INTERCEPT_DISTANCE = 15000  # 截获下滑道距离（单位：米）
    AOA_THRESHOLD = 15.0  # 迎角（Angle of Attack）阈值（单位：度）
    SPEED_UPPER_LIMIT = 150.0  # 速度上限（单位：米/秒）
    SPEED_LOWER_LIMIT = 50.0  # 速度下限（单位：米/秒）
    LOW_ALT_THRESHOLD = 150.0  # 低高度阈值（单位：米，相对于跑道高度）
    CRASH_ALT_THRESHOLD = RUNWAY_ALT - 1  # 坠机高度阈值（低于跑道高度 50 米）

    # CSV 文件路径：用于存储下滑路径、偏差、飞行数据和训练日志
    GLIDE_PATH_CSV = "results/glide_path.csv"  # 下滑路径数据
    GLIDE_DEVIATION_CSV = "results/glide_deviation.csv"  # 路径偏差数据
    FLIGHT_DATA_CSV = "results/flight_data.csv"  # 飞行数据
    TRAINING_LOG_CSV = "results/descent_training_log.csv"  # 训练日志

    def __init__(self):
        # 调用父类构造函数，初始化 Gym 环境
        super().__init__()

        # 定义动作空间：连续动作，6 个维度（俯仰、滚转、油门、襟翼、减速板、方向舵）
        # 动作值范围为 [-1, 1]，数据类型为 32 位浮点数
        self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(6,), dtype=np.float32)

        # 定义观测空间：连续状态，18 个维度（包括位置、速度、姿态等）
        # 状态值范围为 [-∞, ∞]，数据类型为 32 位浮点数
        self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(18,), dtype=np.float32)

        # 初始化环境变量
        self.steps = 0  # 当前回合的步数
        self.current_episode = 0  # 当前回合编号
        self.total_reward = 0  # 当前回合的总奖励
        self.is_on_ground = False  # 飞机是否在地面
        self.last_distance = None  # 上一次到跑道的距离
        self.last_altitude = None  # 上一次的高度
        # self.runway_point = (RUNWAY_LAT_start, RUNWAY_LONG_start)  # 跑道坐标（纬度、经度）
        self.glide_path = []  # 下滑路径点列表
        self.target_point = None  # 当前目标下滑路径点
        self.smoothed_action = [0] * 6  # 平滑后的动作值
        self.last_action = [0] * 6  # 上一次的动作值
        self.flight_trajectory = []  # 飞行轨迹（纬度、经度、高度）
        self.flight_data = []  # 飞行数据记录
        self.dcs_window_active = False  # DCS 窗口是否激活
        self.current_throttle = 0.5  # 当前油门值，初始为 50%
        self.last_speed = None  # 上一次的指示空速
        self.vjoy_initialized = False  # 虚拟摇杆（vJoy）是否初始化

        self._last_data_time = 0
        self.DATA_REFRESH_INTERVAL = 0.02  # 50Hz刷新率
        # 初始化 CSV 文件，用于保存数据
        self._init_csv_files()

        # 生成理想下滑路径
        self._generate_glide_path()

    def reset(self):
        # 重置环境：开始新回合，初始化状态和变量
        self.current_episode += 1  # 回合编号加 1
        self.steps = 0  # 重置步数
        self.total_reward = 0  # 重置总奖励
        self.last_distance = None  # 清空上次距离
        self.last_altitude = None  # 清空上次高度
        self.is_on_ground = False  # 重置地面状态
        self.target_point = None  # 清空目标路径点
        self.flight_trajectory = []  # 清空飞行轨迹
        self.flight_data = []  # 清空飞行数据
        self.smoothed_action = [0] * 6  # 重置平滑动作
        self.last_action = [0] * 6  # 重置上次动作
        self.last_speed = None  # 清空上次速度
        is_crashed = False

        # 获取初始飞行数据
        data = get_dcs_data()

        # # 获取当前 vJoy 控制状态
        # controls = get_current_controls()
        # self.current_throttle = controls['current_throttle']  # 初始化油门值
        # logger.info(f"Reset: Current throttle initialized to {self.current_throttle:.2f}")  # 记录油门初始值

        # 提取当前状态
        self.state = self.extract_state(data)

        # 判断飞机是否在地面
        if data["LLA_balt"] == self.RUNWAY_ALT:
            self.is_on_ground = True    #data["LLA_balt"]是离地高度 跟游戏一致

        # 记录飞行数据和偏差数据
        self._write_flight_data(data)
        self._write_deviation_data(data)

        logger.info(f"Episode {self.current_episode} started, on_ground={self.is_on_ground}")  # 记录回合开始的日志
        return self.state  # 返回初始状态

    def step(self, action):
        # 执行一步：应用动作，更新状态，计算奖励，判断是否结束
        # 统一获取数据
        data = get_dcs_data()
        # 执行动作
        self.take_action(action, data)
        # 提取新状态
        new_state = self.extract_state(data)

        # 计算到跑道的距离和当前高度
        current_distance = geodesic(
            (data["LLA_lat"], data["LLA_long"]), self.runway_point
        ).meters
        current_altitude = data["LLA_balt"]

        # 计算奖励和终止标志
        reward, done_flag = self.calculate_reward(new_state, current_distance, data)

        # 判断回合是否结束
        done = self._is_done(new_state, current_distance, current_altitude) or done_flag

        # 更新总奖励
        self.total_reward += reward

        # 更新环境状态
        self.state = new_state
        self.last_distance = current_distance
        self.last_altitude = current_altitude
        self.steps += 1

        # 记录飞行轨迹
        self.flight_trajectory.append((data["LLA_lat"], data["LLA_long"], data["LLA_balt"]))

        # 记录飞行数据和偏差数据
        self._write_flight_data(data)
        self._write_deviation_data(data)

        # 记录当前步的日志
        logger.info(
            f"Episode: {self.current_episode}, Step: {self.steps}, "
            f"Alt: {current_altitude:.1f}m, IAS: {data['IAS']:.1f}m/s, "
            f"AoA: {data['AoA']:.1f}deg, VS: {data['VV'][2]:.2f}m/s, "
            f"Reward: {reward:.2f}, Total Reward: {self.total_reward:.2f}"
        )

        # 每 50 步或回合结束时绘制轨迹图
        if done or self.steps % 50 == 0:
         self._plot_trajectory()

        # 记录训练日志到 CSV
        with open(TRAINING_LOG_CSV, mode='a', newline='', encoding='utf-8') as file:
            writer = csv.writer(file)
            writer.writerow([
                self.current_episode, self.steps, reward, self.total_reward,
                current_altitude, data["IAS"], new_state[8], data["AoA"],
                data["VV"][2], data["Gears"], data["Flaps"]
            ])
            file.flush()

        return new_state, reward, done, {}  # 返回新状态、奖励、终止标志和额外信息

    def extract_state(self, data):
        # 提取当前状态：将 DCS 数据转换为 18 维状态向量
        current_pos = (data["LLA_lat"], data["LLA_long"])  # 当前位置（纬度、经度）
        nearest_point, path_deviation = self._find_nearest_glide_point(current_pos)  # 找到最近的下滑路径点
        self.target_point = nearest_point  # 更新目标路径点
        dist_to_runway = geodesic(current_pos, self.runway_point).meters  # 计算到跑道的距离
        alt_error = data["LLA_balt"] - nearest_point["alt"]  # 计算高度误差（相对于目标路径点）

        # 计算路径角度误差（飞机航向与跑道方向的夹角）
        target_vec = np.array([RUNWAY_LAT_start - current_pos[0], RUNWAY_LONG_start - current_pos[1]])  # 目标向量
        heading_vec = np.array(
            [math.cos(math.radians(data["heading_deg"])), math.sin(math.radians(data["heading_deg"]))])  # 当前航向向量
        path_angle_error = math.degrees(math.acos(np.clip(
            np.dot(target_vec, heading_vec) / (np.linalg.norm(target_vec) * np.linalg.norm(heading_vec) + 1e-6), -1, 1
        )))  # 计算夹角（弧度转角度）

        # 计算跑道航向误差
        runway_heading_error = min(
            abs(TRUE_HEADING - data["heading_deg"]),
            360 - abs(TRUE_HEADING - data["heading_deg"])
        )

        # 构造状态向量（18 维）
        state = np.array([
            data["LLA_lat"], data["LLA_long"], data["LLA_balt"],  # 纬度、经度、高度
            data["IAS"], data["pitch_deg"], data["bank_deg"],  # 指示空速、俯仰角、滚转角
            data["AoA"], data["VV"][2], heading_error,  # 迎角、垂直速度、航向误差
            alt_error, dist_to_runway, data["Fuel"],  # 高度误差、到跑道距离、燃油
            data["Omega"][0], data["Omega"][1], data["Omega"][2],  # 角速度（x, y, z）
            path_deviation, path_angle_error, runway_heading_error  # 路径偏差、路径角度误差、跑道航向误差
        ], dtype=np.float32)

        # 验证状态向量长度是否正确
        assert len(state) == self.observation_space.shape[
            0], f"State length mismatch: {len(state)} vs {self.observation_space.shape[0]}"
        return state  # 返回状态向量

    def calculate_reward(self, state, current_distance, data):
        # 计算奖励：基于状态、距离和 DCS 数据，评估飞机表现
        reward = 0  # 初始化奖励
        path_deviation = state[15]  # 路径偏差
        path_angle_error = state[16]  # 路径角度误差
        runway_heading_error = state[17]  # 跑道航向误差
        alt_error = state[9]  # 高度误差
        heading_error = state[8]  # 航向误差
        speed_error = abs(state[3] - APPROACH_SPEED)  # 速度误差（相对于进近速度）

        # 权重参数：根据距离调整高度权重
        w_altitude = 1.5 if current_distance < 3000 else 1.0  # 近距离增加高度权重
        w_deviation = 0.7  # 路径偏差权重
        w_heading = 0.4  # 航向权重
        w_speed = 0.3  # 速度权重
        w_runway_heading = 0.5  # 跑道航向权重

        # 距离减少奖励：如果靠近跑道，给予正奖励
        if self.last_distance is not None:
            distance_reduction = max(0, self.last_distance - current_distance)  # 计算距离减少量
            reward += distance_reduction * 0.5  # 每米减少 0.5 分

        # 路径偏差和角度误差惩罚
        reward -= w_deviation * path_deviation * 0.3  # 每米偏差扣分
        reward -= w_deviation * path_angle_error * 0.3  # 每度角度误差扣分

        # 高度误差奖励/惩罚
        alt_threshold = 50.0  # 高度误差阈值
        if abs(alt_error) < alt_threshold:
            reward += w_altitude * 300 * (1 - abs(alt_error) / alt_threshold)  # 高度接近目标，奖励
        else:
            reward -= w_altitude * (abs(alt_error) ** 0.5) * 2  # 高度偏差过大，惩罚

        # 跑道航向误差奖励/惩罚
        heading_threshold = 10.0  # 航向误差阈值
        if runway_heading_error < heading_threshold:
            reward += w_runway_heading * 100 * (1 - runway_heading_error / heading_threshold)  # 航向准确，奖励
        else:
            reward -= w_runway_heading * runway_heading_error * 0.4  # 航向偏差，惩罚

        # 速度误差奖励/惩罚
        speed_threshold = 30.0  # 速度误差阈值
        if speed_error < speed_threshold:
            reward += w_speed * 80 * (1 - speed_error / speed_threshold)  # 速度接近目标，奖励
        else:
            reward -= w_speed * speed_error * 0.2  # 速度偏差过大，惩罚

        # 迎角（AoA）惩罚
        if data["AoA"] > 12.0:
            aoa_penalty = 50 * (data["AoA"] - 12.0) / (AOA_THRESHOLD - 12.0)  # 迎角过高，线性惩罚
            reward -= aoa_penalty
        if data["AoA"] > AOA_THRESHOLD:
            reward -= 50  # 迎角超过阈值，额外惩罚

        # 速度范围检查
        if data["IAS"] < SPEED_LOWER_LIMIT or data["IAS"] > SPEED_UPPER_LIMIT:
            reward -= 20  # 速度超出范围，扣分

        # 坠机检查
        if data["LLA_balt"] < CRASH_ALT_THRESHOLD:
            reward -= 2000  # 高度低于坠机阈值，重罚

        # 距离过远检查
        if current_distance > INTERCEPT_DISTANCE * 1.5:
            reward -= 200  # 距离跑道过远，扣分

        # 固定步惩罚：鼓励快速完成任务
        reward -= 2.0

        return reward  # 返回奖励

    def _is_done(self, state):
        # 判断回合是否结束（终止条件）
        if is_crashed:
            logger.warning("坠机!")  # 坠机
            return True
        if state[2] == self.RUNWAY_ALT and state[3] == 0 and is_crashed == False:
            logger.info("成功着陆!")  # 成功着陆：高度=跑道，速度=0
            self._plot_trajectory()  # 绘制并保存轨迹图
            return True
        if self.steps >= 2000:
            logger.warning("达到最大步数限制!")  # 达到最大步数限制
            return True
        return False  # 未满足终止条件，继续运行


    def take_action(self, action, data):
        # 执行动作：根据输入动作控制飞机的俯仰、滚转、油门等

        try:
            if self.steps == 1:
                set_throttle(0.0)

            # 解析动作：6 个维度（俯仰、滚转、油门、襟翼、减速板、方向舵）
            pitch_action, roll_action, throttle_action, flaps_action, airbrake_action, foot_action = action
            # 获取当前飞机位置
            current_pos = (data["LLA_lat"], data["LLA_long"])
            # 获取当前飞机状态
            current_lat = data["LLA_lat"]
            current_long = data["LLA_long"]
            current_heading = data['heading']






            # 找到最近的下滑路径点和路径偏差
            # nearest_point, path_deviation = self._find_nearest_glide_point(current_pos)
            # 计算高度误差（相对于目标路径点）
            # alt_error = data["LLA_balt"] - nearest_point["alt"]
            # 计算速度误差（相对于进近速度）
            # speed_error = data["IAS"] - APPROACH_SPEED
            # 计算到跑道的距离
            dist_to_runway = geodesic(current_pos, self.runway_point).meters
            # 计算速度变化率（用于减速板控制）
            speed_rate = 0.0
            if self.last_speed is not None:
                speed_rate = (data["IAS"] - self.last_speed) / 0.05  # 速度变化率（每秒）
            self.last_speed = data["IAS"]  # 更新上次速度
            # 计算目标航向（从当前位置指向跑道）
            # lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
            # lat2, lon2 = math.radians(RUNWAY_LAT), math.radians(RUNWAY_LONG)
            # dlon = lon2 - lon1
            # y = math.sin(dlon) * math.cos(lat2)
            # x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
            # target_heading = math.degrees(math.atan2(y, x)) % 360
            # 计算航向误差
            # heading_error = min(
            #     abs(target_heading - data["heading_deg"]),
            #     360 - abs(target_heading - data["heading_deg"])
            # )
            # heading_error_signed = target_heading - data["heading_deg"]

            # 计算跑道航向误差
            # runway_heading_error_signed = TRUE_HEADING - data["heading_deg"]

            # 计算目标vertical速度（基于下滑角）
            target_vertical_speed = -data["IAS"] * math.sin(GLIDE_ANGLE)
            vertical_speed_error = data["VV"][2] - target_vertical_speed

            # 平滑动作：避免控制输入过于突变
            smooth_factor = 0.2
            for i in range(6):
                self.smoothed_action[i] = (1 - smooth_factor) * action[i] + smooth_factor * self.last_action[i]
                self.last_action[i] = self.smoothed_action[i]

            # 使用平滑后的动作
            pitch_action, roll_action, throttle_action, flaps_action, airbrake_action, foot_action = self.smoothed_action

            # 记录动作前的油门状态
            logger.info(
                f"Before action: throttle={self.current_throttle:.2f}, vJoy={get_current_controls()['current_throttle']:.2f}"
            )

            # 俯仰控制：基于高度误差和垂直速度误差
            pitch_kp_alt = 0.0015
            pitch_kp_vs = 0.05
            pitch_action_weight = 0.03
            # pitch_value = pitch_kp_alt * alt_error + pitch_kp_vs * vertical_speed_error + pitch_action_weight * pitch_action
            pitch_value = pitch_kp_alt * alt_error - pitch_action_weight * pitch_action
            pitch_value = np.clip(pitch_value, -1, 1)  # 放宽范围
            logger.info(
                f"Pitch control (value={pitch_value:.2f}): Alt error={alt_error:.1f}m, VS error={vertical_speed_error:.1f}m/s."
            )
            set_pitch(-pitch_value)  # 设置俯仰控制
            time.sleep(0.3)
            set_pitch(0.2*pitch_value)#基础回正


            # 滚转控制：基于航向误差
            roll_action_weight = 0.3
            roll_kp = 0.05  # 滚转比例系数
            if dist_to_runway > 5000:
                roll_value = roll_kp * runway_heading_error_signed+roll_action_weight*roll_action  # 远距离使用跑道航向
            else:
                roll_value = roll_kp * runway_heading_error_signed+roll_action_weight*roll_action # 远距离使用跑道航向
                # roll_value = roll_kp * heading_error_signed  # 近距离使用目标航向
            roll_value = np.clip(roll_value, -0.5, 0.5)  # 限制滚转控制范围
            logger.info(
                f"Roll control (value={roll_value:.2f}): Runway heading error={runway_heading_error_signed:.1f}deg, Path heading error={heading_error_signed:.1f}deg."
            )
            set_roll(roll_value)  # 设置滚转控制
            time.sleep(0.3)
            set_roll(-0.3*roll_value)#基础回正

            # # 油门控制：基于速度误差
            # throttle_scale = 0.1 * (1 + abs(speed_error) / 10.0)  # 动态调整油门变化幅度
            # throttle_scale = min(throttle_scale, 0.25)  # 限制最大变化幅度
            # desired_throttle_change = throttle_action * throttle_scale
            # target_throttle = np.clip(self.current_throttle + desired_throttle_change, 0.0, 1.0)  # 计算目标油门值
            # if speed_error > 5.0:
            #     logger.info(
            #         f"Decreasing throttle to {target_throttle:.2f}: IAS={data['IAS']:.1f}m/s, Speed error={speed_error:.1f}m/s."
            #     )
            #     throttle_down(abs(desired_throttle_change))  # 减小油门
            #     self.current_throttle = target_throttle
            # elif speed_error < -5.0:
            #     logger.info(
            #         f"Increasing throttle to {target_throttle:.2f}: IAS={data['IAS']:.1f}m/s, Speed error={speed_error:.1f}m/s."
            #     )
            #     throttle_up(abs(desired_throttle_change))  # 增大油门
            #     self.current_throttle = target_throttle
            # else:
            #     logger.info(
            #         f"Maintaining throttle at {target_throttle:.2f}: IAS={data['IAS']:.1f}m/s, Speed error={speed_error:.1f}m/s."
            #     )
            #     set_throttle(target_throttle)  # 保持油门
            #     self.current_throttle = target_throttle
            #
            # # 方向舵控制：基于航向误差和滚转角
            # rudder_value = np.clip(foot_action, -1.0, 1.0)
            # rudder_kp_heading = 0.02  # 航向误差比例系数
            # rudder_kp_roll = 0.1  # 滚转角比例系数
            # rudder_scale = 0.3 * (1 + heading_error / 15.0)  # 动态调整方向舵幅度
            # rudder_scale = min(rudder_scale, 0.4)  # 限制最大幅度
            # if dist_to_runway > 5000 and abs(runway_heading_error_signed) > 3.0:
            #     rudder_value = rudder_kp_heading * runway_heading_error_signed + rudder_kp_roll * data["bank_deg"]
            #     rudder_value = np.clip(rudder_value, -rudder_scale, rudder_scale)
            #     logger.info(
            #         f"Rudder (value={rudder_value:.2f}): Runway heading error={runway_heading_error_signed:.1f}deg, Bank={data['bank_deg']:.1f}deg, Path deviation={path_deviation:.1f}m."
            #     )
            # elif heading_error > 3.0:
            #     rudder_value = rudder_kp_heading * heading_error_signed + rudder_kp_roll * data["bank_deg"]
            #     rudder_value = np.clip(rudder_value, -rudder_scale, rudder_scale)
            #     logger.info(
            #         f"Rudder (value={rudder_value:.2f}): Path heading error={heading_error_signed:.1f}deg, Bank={data['bank_deg']:.1f}deg, Path deviation={path_deviation:.1f}m."
            #     )
            # else:
            #     rudder_value = 0.0
            #     logger.info(
            #         f"No rudder input (value={rudder_value:.2f}): Heading error={heading_error:.1f}deg, Path deviation={path_deviation:.1f}m."
            #     )
            # set_rudder(rudder_value)  # 设置方向舵控制
            #
            # # 减速板控制：基于速度和速度变化率
            # speed_threshold_high = APPROACH_SPEED + 5  # 速度上限阈值
            # speed_threshold_low = APPROACH_SPEED - 5  # 速度下限阈值
            # speed_rate_threshold = 2.0  # 速度变化率阈值
            # current_airbrake = data.get("AirBrake", 0)  # 获取当前减速板状态
            # if (data["IAS"] > speed_threshold_high or speed_rate > speed_rate_threshold) and current_airbrake == 0:
            #     logger.info(f"Deploying airbrake: IAS={data['IAS']:.1f}m/s, Speed rate={speed_rate:.2f}m/s^2.")
            #     airbrake_on_control()  # 打开减速板
            # elif data["IAS"] < speed_threshold_low and current_airbrake == 1:
            #     logger.info(f"Retracting airbrake: IAS={data['IAS']:.1f}m/s, Speed rate={speed_rate:.2f}m/s^2.")
            #     airbrake_off_control()  # 关闭减速板

            #
            # # 襟翼控制：基于高度和当前襟翼状态
            # if flaps_action > 0.1 and data["LLA_balt"] - RUNWAY_ALT < LOW_ALT_THRESHOLD and data["Flaps"] < 0.9:
            #     logger.info(f"Deploying flaps: Altitude={data['LLA_balt']:.1f}m.")
            #     flapsland_control()  # 放下襟翼
            #
            #
            # # 起落架控制：基于高度和当前起落架状态
            # if data["LLA_balt"] - RUNWAY_ALT < LOW_ALT_THRESHOLD and data["Gears"] < 0.9:
            #     logger.info(f"Lowering gear: Altitude={data['LLA_balt']:.1f}m.")
            #     gearson_control()  # 放下起落架
            #
            # # 高迎角恢复：当迎角过高时强制俯仰下压
            # if data["AoA"] > AOA_THRESHOLD * 0.7:
            #     pitch_value = -0.5  # 俯仰下压
            #     logger.info(f"High AoA recovery: AoA={data['AoA']:.1f}deg.")
            #     set_pitch(pitch_value)
            #     time.sleep(0.2)
            #     set_pitch(0.0)  # 恢复中立俯仰

            # 记录动作后的油门状态
            logger.info(
                f"After action: throttle={self.current_throttle:.2f}, vJoy={get_current_controls()['current_throttle']:.2f}"
            )
        except Exception as e:
            logger.error(f"Error in _take_action: {str(e)}")  # 记录动作执行中的错误
            raise



    def _init_csv_files(self):
        # 初始化 CSV 文件：创建结果目录并写入文件头
        os.makedirs("results", exist_ok=True)  # 创建 results 目录（如果不存在）

        # 初始化训练日志 CSV 文件
        with open(TRAINING_LOG_CSV, mode='w', newline='', encoding='utf-8') as file:
            writer = csv.writer(file)
            writer.writerow([
                "Episode", "Step", "Reward", "Total Reward", "Alt", "IAS",
                "Heading Error", "AoA", "Vertical Speed", "Gears", "Flaps"
            ])  # 写入表头：回合、步数、奖励、总奖励、高度、指示空速、航向误差、迎角、垂直速度、起落架、襟翼
            file.flush()  # 刷新文件缓冲区，确保写入

        # 初始化飞行数据 CSV 文件
        with open(FLIGHT_DATA_CSV, mode='w', newline='', encoding='utf-8') as file:
            writer = csv.writer(file)
            writer.writerow([
                "Episode", "Step", "Long", "Lat", "Alt", "IAS", "AoA",
                "Pitch", "Bank", "Heading", "Vertical Speed", "Gears", "Flaps",
                "AirBrake", "Fuel", "Dist to Runway"
            ])  # 写入表头：回合、步数、经度、纬度、高度、指示空速、迎角、俯仰角、滚转角、航向、垂直速度、起落架、襟翼、减速板、燃油、到跑道距离
            file.flush()

        # 初始化路径偏差 CSV 文件
        with open(GLIDE_DEVIATION_CSV, mode='w', newline='', encoding='utf-8') as file:
            writer = csv.writer(file)
            writer.writerow([
                "Episode", "Step", "Long", "Lat", "Alt", "Path Deviation",
                "Alt Error", "Path Angle Error"
            ])  # 写入表头：回合、步数、经度、纬度、高度、路径偏差、高度误差、路径角度误差
            file.flush()

    def _generate_glide_path(self):
        # 生成理想下滑路径：从跑道起点向外延伸，计算路径点坐标和高度
        # 使用 geodesic 库计算地理坐标，基于下滑角和截获距离
        gsip = geodesic(kilometers=INTERCEPT_DISTANCE / 1000).destination(
            self.runway_point, RUNWAY_HEADING + 180
        )  # 计算下滑道截获点（GSIP），从跑道向反方向（航向+180度）延伸 15 公里
        gsip_alt = RUNWAY_ALT + INTERCEPT_DISTANCE * math.tan(GLIDE_ANGLE)  # 计算截获点高度：跑道高度 + 距离 * tan(下滑角)
        self.glide_path = []  # 清空下滑路径列表
        num_points = 15 # 定义路径点数量（这里为 15 个点）
        distances = np.linspace(0, INTERCEPT_DISTANCE, num_points)  # 在 0 到截获距离之间线性分布点
        for dist in distances:
            point = geodesic(kilometers=dist / 1000).destination(
                (gsip.latitude, gsip.longitude), (RUNWAY_HEADING) % 360
            )  # 从截获点向跑道方向（航向）计算路径点坐标
            alt = RUNWAY_ALT + (INTERCEPT_DISTANCE - dist) * math.tan(GLIDE_ANGLE)  # 计算路径点高度
            self.glide_path.append({"lat": point.latitude, "lon": point.longitude, "alt": alt})  # 添加路径点（纬度、经度、高度）

        # 将下滑路径保存到 CSV 文件
        with open(GLIDE_PATH_CSV, mode='w', newline='', encoding='utf-8') as file:
            writer = csv.writer(file)
            writer.writerow(["Lat", "Lon", "Alt"])  # 写入表头：纬度、经度、高度
            for p in self.glide_path:
                writer.writerow([p["lat"], p["lon"], p["alt"]])  # 写入每个路径点的坐标和高度
            file.flush()  # 刷新文件缓冲区
        logger.info(f"Generated glide path with {num_points} points, saved to {GLIDE_PATH_CSV}")  # 记录生成路径的日志

    def _plot_trajectory(self):
        # 绘制飞行轨迹和高度曲线，保存为 PNG 文件
        if not self.flight_trajectory:
            return  # 如果没有飞行轨迹数据，直接返回

        # 提取飞行轨迹和下滑路径的坐标
        traj_lats, traj_lons, traj_alts = zip(*self.flight_trajectory)  # 解压飞行轨迹（纬度、经度、高度）
        path_lats = [p["lat"] for p in self.glide_path]  # 下滑路径纬度
        path_lons = [p["lon"] for p in self.glide_path]  # 下滑路径经度
        path_alts = [p["alt"] for p in self.glide_path]  # 下滑路径高度

        # 绘制二维轨迹图（经度-纬度）
        plt.figure(figsize=(12, 7))  # 设置图形大小
        plt.plot(path_lons, path_lats, 'b-', label='Glide Path')  # 绘制下滑路径（蓝色实线）
        plt.plot(traj_lons, traj_lats, 'g--', label='Flight Trajectory')  # 绘制飞行轨迹（绿色虚线）
        plt.scatter([RUNWAY_LONG], [RUNWAY_LAT], color='red', label='Runway')  # 标记跑道位置（红色点）
        plt.xlabel('Longitude')  # X 轴标签：经度
        plt.ylabel('Latitude')  # Y 轴标签：纬度
        plt.title(f'Flight Trajectory vs Glide Path (Episode {self.current_episode})')  # 图标题：轨迹对比
        plt.legend()  # 显示图例
        plt.grid(True)  # 显示网格
        plt.savefig(f'results/trajectory_episode_{self.current_episode}.png')  # 保存轨迹图
        plt.close()  # 关闭图形，释放内存

        # 绘制高度曲线图
        plt.figure(figsize=(12, 4))  # 设置图形大小
        plt.plot(range(len(path_alts)), path_alts, 'b-', label='Glide Path Altitude')  # 绘制下滑路径高度（蓝色实线）
        plt.plot(range(len(traj_alts)), traj_alts, 'g--', label='Flight Altitude')  # 绘制飞行高度（绿色虚线）
        plt.axhline(RUNWAY_ALT + LOW_ALT_THRESHOLD, color='purple', linestyle='--',
                    label='350m Altitude')  # 绘制 350 米高度线（紫色虚线）
        plt.xlabel('Path Point Index')  # X 轴标签：路径点索引
        plt.ylabel('Altitude (m)')  # Y 轴标签：高度（米）
        plt.title(f'Altitude Profile (Episode {self.current_episode})')  # 图标题：高度曲线
        plt.legend()  # 显示图例
        plt.grid(True)  # 显示网格
        plt.savefig(f'results/altitude_profile_episode_{self.current_episode}.png')  # 保存高度图
        plt.close()  # 关闭图形
        logger.info(f"Trajectory and altitude profile saved for episode {self.current_episode}")  # 记录保存成功的日志


    def _get_dcs_data_with_retry(self, max_retries=3):
        # 获取 DCS 数据，带重试机制以处理数据获取失败
        for attempt in range(max_retries):
            try:
                data = get_dcs_data()  # 调用自定义模块获取 DCS 数据
                if data is not None:
                    return data  # 数据获取成功，返回数据
                retry_delay = 0.1 + 0.1 * attempt  # 计算重试延迟（随尝试baar次增加）
                logger.warning(
                    f"Data acquisition failed (attempt {attempt + 1}/{max_retries}), retrying after {retry_delay}s")
                time.sleep(retry_delay)  # 等待后重试
            except Exception as e:
                logger.error(f"Data acquisition error (attempt {attempt + 1}/{max_retries}): {str(e)}")  # 记录错误
                time.sleep(retry_delay)
        return None  # 重试失败，返回 None

    def _find_nearest_glide_point(self, current_pos):
        # 找到距离飞机当前位置最近的下滑路径点
        min_dist_to_runway = float('inf')  # 初始化到跑道的最小距离为无穷大
        nearest_point = None  # 初始化最近路径点
        path_deviation = float('inf')  # 初始化路径偏差为无穷大
        for p in self.glide_path:
            dist_to_runway = geodesic((p["lat"], p["lon"]), self.runway_point).meters  # 计算路径点到跑道的距离
            dist_to_aircraft = geodesic(current_pos, (p["lat"], p["lon"])).meters  # 计算路径点到飞机的距离
            if dist_to_runway < min_dist_to_runway and dist_to_aircraft < 5000:
                min_dist_to_runway = dist_to_runway  # 更新最小跑道距离
                nearest_point = p  # 更新最近路径点
                path_deviation = dist_to_aircraft  # 更新路径偏差
        if nearest_point is None:
            min_dist = float('inf')  # 如果没有找到满足条件的点，寻找全局最近点
            for p in self.glide_path:
                dist = geodesic(current_pos, (p["lat"], p["lon"])).meters
                if dist < min_dist:
                    min_dist = dist
                    nearest_point = p
                    path_deviation = min_dist
        return nearest_point, path_deviation  # 返回最近路径点和路径偏差




    def _write_flight_data(self, data):
        # 记录飞行数据到 CSV 文件
        dist_to_runway = geodesic(
            (data["LLA_lat"], data["LLA_long"]), self.runway_point
        ).meters  # 计算到跑道的距离
        with open(FLIGHT_DATA_CSV, mode='a', newline='', encoding='utf-8') as file:
            writer = csv.writer(file)
            writer.writerow([
                self.current_episode, self.steps,
                data["LLA_long"], data["LLA_lat"], data["LLA_balt"],
                data["IAS"], data["AoA"], data["pitch_deg"], data["bank_deg"],
                data["heading_deg"], data["VV"][2], data["Gears"], data["Flaps"],
                data["AirBrake"], data["Fuel"], dist_to_runway
            ])  # 写入飞行数据
            file.flush()

    def _write_deviation_data(self, data):
        # 记录路径偏差数据到 CSV 文件
        current_pos = (data["LLA_lat"], data["LLA_long"])  # 当前位置
        nearest_point, path_deviation = self._find_nearest_glide_point(current_pos)  # 找到最近路径点
        alt_error = data["LLA_balt"] - nearest_point["alt"]  # 计算高度误差
        target_vec = np.array([RUNWAY_LAT - current_pos[0], RUNWAY_LONG - current_pos[1]])  # 目标向量
        heading_vec = np.array(
            [math.cos(math.radians(data["heading_deg"])), math.sin(math.radians(data["heading_deg"]))])  # 当前航向向量
        path_angle_error = math.degrees(math.acos(np.clip(
            np.dot(target_vec, heading_vec) / (np.linalg.norm(target_vec) * np.linalg.norm(heading_vec) + 1e-6), -1, 1
        )))  # 计算路径角度误差
        with open(GLIDE_DEVIATION_CSV, mode='a', newline='', encoding='utf-8') as file:
            writer = csv.writer(file)
            writer.writerow([
                self.current_episode, self.steps,
                data["LLA_long"], data["LLA_lat"], data["LLA_balt"],
                path_deviation, alt_error, path_angle_error
            ])  # 写入偏差数据
            file.flush()

    def align_to_runway(self, current_lat, current_long, current_heading):
        """
        完全基于跑道起点和终点坐标的对准函数，确保飞机在跑道中心线延伸线上
        参数:
            current_lat: 飞机当前纬度
            current_long: 飞机当前经度
            current_heading: 飞机当前航向(度)
        返回:
            无返回值，直接通过set_rudder设置脚舵动作
        """

        # 将经纬度转换为平面坐标（简化投影）
        def project(lat, lon):
            # 使用跑道起点作为参考点
            ref_lat = self.RUNWAY_LAT_start
            ref_lon = self.RUNWAY_LONG_start
            # 简化的平面投影（适用于小范围）
            x = (lon - ref_lon) * 111320 * math.cos(math.radians(ref_lat))
            y = (lat - ref_lat) * 111320  # 1度纬度≈111,320米
            return (x, y)

        # 计算跑道方向向量
        runway_start = project(self.RUNWAY_LAT_start, self.RUNWAY_LONG_start)
        runway_end = project(self.RUNWAY_LAT_end, self.RUNWAY_LONG_end)
        runway_vec = (runway_end[0] - runway_start[0],
                      runway_end[1] - runway_start[1])

        # 计算跑道航向（用于调试和航向修正）
        runway_heading = math.degrees(math.atan2(runway_vec[1], runway_vec[0])) % 360

        # 计算飞机位置相对于跑道起点的向量
        aircraft_pos = project(current_lat, current_long)
        rel_pos = (aircraft_pos[0] - runway_start[0],
                   aircraft_pos[1] - runway_start[1])

        # 计算横向偏差（垂直于跑道方向的投影）
        # 跑道方向的垂直向量（左转90度）
        perpendicular_vec = (-runway_vec[1], runway_vec[0])

        # 计算飞机在垂直跑道方向上的投影长度（即横向偏差）
        cross_track_error = (rel_pos[0] * perpendicular_vec[0] +
                             rel_pos[1] * perpendicular_vec[1])

        # 计算航向偏差（当前航向与跑道航向的差值）
        heading_error = (current_heading - runway_heading + 180) % 360 - 180

        # 计算需要的脚舵量
        # 横向偏差控制（主要修正）
        rudder_cross_track = 0.0
        if abs(cross_track_error) > 10:  # 10米阈值
            # 比例控制，gain是控制增益，需要调整
            gain = 0.02
            rudder_cross_track = -gain * cross_track_error
            # 限制最大舵量
            rudder_cross_track = max(-0.5, min(0.5, rudder_cross_track))

        # 航向偏差控制（辅助修正）
        rudder_heading = 0.0
        if abs(heading_error) > 2.0:  # 2度阈值
            gain = 0.05  # 航向控制增益
            rudder_heading = gain * heading_error
            rudder_heading = max(-0.3, min(0.3, rudder_heading))

        # 综合舵量
        total_rudder = rudder_cross_track + rudder_heading
        total_rudder = max(-1.0, min(1.0, total_rudder))

        # 应用脚舵
        set_rudder(total_rudder)

        # 调试信息（可选）
        if hasattr(self, 'debug') and self.debug:
            print(f"Runway heading: {runway_heading:.1f}°, "
                  f"Cross track error: {cross_track_error:.1f}m, "
                  f"Heading error: {heading_error:.1f}°, "
                  f"Rudder: {total_rudder:.2f}")